#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "std_msgs/msg/float64.hpp"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"

#include "general_ctrl/PositionPID.hpp"

#include <chrono>
#include <cmath>
#include <functional>
#include <memory>

class CascadePIDCtrl : public rclcpp::Node
{
    using IMUMsg = sensor_msgs::msg::Imu;
    using WheelSpeedMsg = std_msgs::msg::Float64;
    using MotorForceMsg = std_msgs::msg::Float64;

  public:
    explicit CascadePIDCtrl() : Node("CascadePIDCtrl")
    {
        using namespace std::chrono_literals;

        leftMotorForcePub =
            create_publisher<MotorForceMsg>("/left_motor_force", rclcpp::SystemDefaultsQoS().reliable());
        rightMotorForcePub =
            create_publisher<MotorForceMsg>("/right_motor_force", rclcpp::SystemDefaultsQoS().reliable());

        IMUSub =
            create_subscription<IMUMsg>("/IMU", 10, std::bind(&CascadePIDCtrl::OnIMUMsg, this, std::placeholders::_1));

        leftWheelSpeedSub = create_subscription<WheelSpeedMsg>(
            "/left_motor_speed", 10, [this](const WheelSpeedMsg &msg) { leftSpeed = msg.data; });

        rightWheelSpeedSub = create_subscription<WheelSpeedMsg>(
            "/right_motor_speed", 10, [this](const WheelSpeedMsg &msg) { rightSpeed = msg.data; });

        timer = create_wall_timer(10ms, std::bind(&CascadePIDCtrl::OnTick, this));

        paramHandle =
            add_post_set_parameters_callback(std::bind(&CascadePIDCtrl::OnParamsChange, this, std::placeholders::_1));

        DeclareParams();
        LoadParams();
    }

  private:
    rclcpp::Subscription<IMUMsg>::SharedPtr IMUSub;
    rclcpp::Subscription<WheelSpeedMsg>::SharedPtr leftWheelSpeedSub;
    rclcpp::Subscription<WheelSpeedMsg>::SharedPtr rightWheelSpeedSub;
    rclcpp::Publisher<MotorForceMsg>::SharedPtr leftMotorForcePub;
    rclcpp::Publisher<MotorForceMsg>::SharedPtr rightMotorForcePub;
    rclcpp::TimerBase::SharedPtr timer;
    PostSetParametersCallbackHandle::SharedPtr paramHandle;

    double rightSpeed = 0;
    double leftSpeed = 0;
    double roll = 0;
    double yaw = 0;
    double pitch = 0;
    double gyro[3] = {0, 0, 0};
    double acc[3] = {0, 0, 0};
    double ctrlLineSpeed = 0;
    double ctrlAngularSpeed = 0;

    GeneralCtrl::PositionPID gyroPID;
    GeneralCtrl::PositionPID anglePID;
    GeneralCtrl::PositionPID rotatePID;
    GeneralCtrl::PositionPID speedPID;

    struct Config
    {
        double maxAngle = 40.0 / 180 * M_PI;
        double errorAngle = 90.0 / 180 * M_PI;
        double maxSpeed = 200;
        double errorSpeed = 200;
        double maxOutput = 100;
    };
    Config config;

    double Raind2Deg(double r) { return r / M_PI * 180; }
    double Deg2Raind(double d) { return d / 180 * M_PI; }

    void CheckValueRangeAndExit(double val, double min, double max, std::string message)
    {
        if (val < min || val > max)
        {
            RCLCPP_ERROR(get_logger(), "Value %s Out of Range", message.c_str());
            // rclcpp::shutdown();
        }
    }

    double ABSClamp(double val, double range)
    {
        val = val > range ? range : val;
        val = val < -range ? -range : val;
        return val;
    }

    void OnTick()
    {
        double avrSpeed = (leftSpeed + rightSpeed) / 2;
        double speedError = avrSpeed - ctrlLineSpeed;
        // speed loop is positive
        double speedCtrlAngle = -speedPID.GetControl(speedError, 10);
        speedCtrlAngle = ABSClamp(speedCtrlAngle, config.maxAngle);

        CheckValueRangeAndExit(pitch, -config.errorAngle, config.errorAngle, "angle");

        // angle control loop
        // forward is positive(+),backward is negative(-)
        double angleError = pitch - speedCtrlAngle;
        // double angleError = angle - input.move;
        double ctrl = anglePID.GetControl(angleError, 10);

        // gyro control loop
        // double gyroError = gyro[0] - gyroCtrl;
        // double ctrl = gyroPID.GetControl(gyroError, 10);
        // ctrl = ABSClamp(ctrl, config.maxOutput);

        // rotate loop
        double rotateCtrl = rotatePID.GetControl(gyro[2] - ctrlAngularSpeed, 10);
        rotateCtrl = ABSClamp(rotateCtrl, config.maxOutput * 0.2f);

        MotorForceMsg forceMsg;
        forceMsg.data = ctrl - rotateCtrl;
        leftMotorForcePub->publish(forceMsg);
        forceMsg.data = ctrl + rotateCtrl;
        rightMotorForcePub->publish(forceMsg);
    }

    void OnIMUMsg(const IMUMsg &msg)
    {
        tf2::Quaternion qua(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w);
        // tf2::Matrix3x3(qua).getRPY(roll, pitch, yaw);
        tf2::Matrix3x3(qua).getEulerYPR(yaw, pitch, roll);
        // RCLCPP_INFO(get_logger(), "Angle:%.3f,%.3f,%.3f", Raind2Deg(roll), Raind2Deg(pitch), Raind2Deg(yaw));
        gyro[0] = msg.angular_velocity.x;
        gyro[1] = msg.angular_velocity.y;
        gyro[2] = msg.angular_velocity.z;
        // RCLCPP_INFO(get_logger(), "Gyro:%f,%f,%f", gyro[0], gyro[1], gyro[2]);
        acc[0] = msg.linear_acceleration.x;
        acc[1] = msg.linear_acceleration.y;
        acc[2] = msg.linear_acceleration.z;
        // RCLCPP_INFO(get_logger(), "Angle:%f,%f,%f", acc[0], acc[1], acc[2]);
    }

    void OnParamsChange(const std::vector<rclcpp::Parameter> &parameters)
    {
        for (auto param : parameters)
        {
            auto name = param.get_name();
            if (name == "AngleP")
            {
                anglePID.SetP(param.as_double());
            }
            else if (name == "AngleI")
            {
                anglePID.SetI(param.as_double());
            }
            else if (name == "AngleD")
            {
                anglePID.SetD(param.as_double());
            }
            else if (name == "SpeedP")
            {
                speedPID.SetP(param.as_double());
            }
            else if (name == "SpeedI")
            {
                speedPID.SetI(param.as_double());
            }
            else if (name == "SpeedD")
            {
                speedPID.SetD(param.as_double());
            }
            else if (name == "RotateP")
            {
                rotatePID.SetP(param.as_double());
            }
            else if (name == "RotateI")
            {
                rotatePID.SetI(param.as_double());
            }
            else if (name == "RotateD")
            {
                rotatePID.SetD(param.as_double());
            }
        }
    }

    void DeclareParams()
    {
        declare_parameter("AngleP", double{0});
        declare_parameter("AngleI", double{0});
        declare_parameter("AngleD", double{0});

        declare_parameter("SpeedP", double{0});
        declare_parameter("SpeedI", double{0});
        declare_parameter("SpeedD", double{0});

        declare_parameter("RotateP", double{0});
        declare_parameter("RotateI", double{0});
        declare_parameter("RotateD", double{0});
    }

    void LoadParams()
    {
        anglePID.SetPID(get_parameter_or<double>("AngleP", 0), get_parameter_or<double>("AngleI", 0),
                        get_parameter_or<double>("AngleD", 0));
        speedPID.SetPID(get_parameter_or<double>("SpeedP", 0), get_parameter_or<double>("SpeedI", 0),
                        get_parameter_or<double>("SpeedD", 0));
        rotatePID.SetPID(get_parameter_or<double>("RotateP", 0), get_parameter_or<double>("RotateI", 0),
                         get_parameter_or<double>("RotateD", 0));
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<CascadePIDCtrl>());
    rclcpp::shutdown();
    return 0;
}
